#pragma once
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include "Pose3D.h"
#include <Eigen/Eigen>
using namespace pcl;
typedef pcl::PointXYZ PointIoType;

struct Correspondence_PointCloud {
	pcl::PointCloud<PointIoType>::Ptr target = pcl::PointCloud<PointIoType>::Ptr(new pcl::PointCloud<PointIoType>);
	pcl::PointCloud<PointIoType>::Ptr source = pcl::PointCloud<PointIoType>::Ptr(new pcl::PointCloud<PointIoType>);
};

class MPointCloudMap
{
public:
	void setKdtree(const pcl::PointCloud<pcl::PointXYZ>::Ptr map);
	pcl::PointCloud<pcl::PointXYZ>::Ptr initMap(const string bin_input_path);
	pcl::PointCloud<PointIoType>::Ptr getFromBin(const string bin_input_path);
	double likelihoodCompute(const Pose3D & pose, const pcl::PointCloud<pcl::PointXYZ>::Ptr observation, bool whether_cut = false);
	double likelihoodComputeWithPointCloudSave(const Pose3D & pose,
		const pcl::PointCloud<pcl::PointXYZ>::Ptr observation,
		bool whether_cut = false);
	Pose3D doComputeWithParticleShift(const Pose3D & pose, const int& index, pcl::PointCloud<pcl::PointXYZ >::Ptr observation,
		 const int& shift_particle_function);
	Pose3D likelihoodComputeWithParticleShift(const Pose3D & pose,
		const pcl::PointCloud<pcl::PointXYZ>::Ptr observation,
		const int& shift_particle_function, bool whether_cut = false);
	void computeShift(const pcl::PointCloud<pcl::PointXYZ>::Ptr source_corr, 
		const pcl::PointCloud<pcl::PointXYZ>::Ptr target_corr, 
		Eigen::Matrix4f &transformation_matrix,
		const int& shift_particle_function);
	void computeParticleShift(pcl::PointCloud<pcl::PointXYZ>::Ptr observation,pcl::PointCloud<pcl::PointXYZ>::Ptr source_corr,
		pcl::PointCloud<pcl::PointXYZ>::Ptr target_corr,
		Eigen::Matrix4f &transformation_matrix,
		const int& shift_particle_function);
	void clearCorrespondenceCloudPoint();
	pcl::PointCloud<pcl::PointXYZ>::Ptr getMapData();
	double europeanDistance(const PointIoType& a,const PointIoType& b);
	MPointCloudMap();
	~MPointCloudMap();

private:
	double cut_range_x{ 25.0 }, cut_range_y{25.0};

	//use kdtree to find the close point from shape to model
	pcl::PointCloud<PointIoType>::Ptr MyMap = pcl::PointCloud<PointIoType>::Ptr(new pcl::PointCloud<PointIoType>);
	pcl::KdTreeFLANN<PointIoType> mKdtree;
	vector<Correspondence_PointCloud> m_corr;
	
};

